#pragma once
#include <opencv2/opencv.hpp>
#include "../udp/udp.h"

struct RouteNode {
    int label = 0;
    RouteNode* next = nullptr;

    RouteNode() = default;

    RouteNode(int label) {
        this->label = label;
        this->next = nullptr;
    }

    bool operator==(RouteNode& other) {
        return this->label == other.label;
    }
};


class SubController {
public:
    SubController(std::vector<int>& route);
    ~SubController();
    void update(int currentLabel);

private:
    void match(int label);
    enum class STATE{Double, Single};
    STATE state = STATE::Double;
    RouteNode* currentNode;
    RouteNode* nextNode;
    RouteNode* head = new RouteNode(-1);
};

class Controller {
public:
    Controller() = default;
    ~Controller() = default;
    void init(std::vector<std::vector<int>>& routes);
    void update();
    void sendCommand();
    
    UDP udp;

private:
    std::map<int, SubController*> subcontrollers;
    uint num;      
};

